#include "seat_conver.hpp"

#include <transforms3d/transforms3d.h>
inline double my_abs(double a)
{
    if(a>=0)return a;
    else return -a;
}

void find_seat::load_seat_pcd(std::string filename)
{
    if (pcl::io::loadPCDFile<pcl::PointXYZ> ("seat0.pcd", init_seat_cloud) == -1) {
        PCL_ERROR ("Failed to load init point cloud data.\n");
    }
    init_tree.setInputCloud(init_seat_cloud.makeShared());
}
void find_seat::set_input_coud(pcl::PointCloud<pcl::PointXYZ> &cur_cloud)
{
    std::size_t size;
    int back_index_L,back_index_R;
    double max1,max2,tempf;
    size=cur_cloud.points.size();
    max1=10;max2=10;
    for(int i=0;i<size;i++)
     {
        cur_cloud.points[i].x+=-seat_sf.x;
        if(cur_cloud.points[i].x<0)
        {
            tempf=my_abs(cur_cloud.points[i].y-0.5);
            if(tempf<max1)
            {
                max1=tempf; back_index_L=i;
            }
            tempf=my_abs(cur_cloud.points[i].y+0.5);
            if(tempf<max2)
            {
                max2=tempf; back_index_R=i;
            }
        }
          
     }
    std::vector<int> indices;
    std::vector<float> dist;
   
   // dist=0.2;
     init_tree.nearestKSearch(cur_cloud.points[back_index_L],1,indices,dist);
    std::cout<<"size="<<indices.size()<<"  "<<indices[0]<<" x="<<init_seat_cloud.points[indices[0]].x<<" "<<init_seat_cloud.points[indices[0]].y<<std::endl;
    std::cout<<"indexL="<<back_index_L<<" "<<cur_cloud.points[back_index_L].x<<" " <<cur_cloud.points[back_index_L].y<<std::endl;
    std::cout<<"dist="<<dist[0]<<std::endl;
    seat_sf.x+= cur_cloud.points[back_index_L].x-init_seat_cloud.points[indices[0]].x;
//    std::cout<<"indexR="<<back_index_R<<" "<<cur_cloud.points[back_index_R].x<<" " <<cur_cloud.points[back_index_R].y<<std::endl;   
    // for(int i=back_index_L;i<back_index_R+1;i++)  
    //   std::cout<<"index="<<i<<" "<<cur_cloud.points[i].x<<" " <<cur_cloud.points[i].y<<std::endl;
}

void find_seat::set_seat_zero(void)
{
    seat_sf.x=0;
    seat_sf.y=0;
    seat_sf.wmga=0;
}


// int
//         nearestKSearch (const PointT &point, int k, 
//                         Indices &k_indices,
//                         std::vector<float> &k_sqr_distances) const override;